﻿#include <vtkSmartPointer.h>
#include <vtkPlane.h>
#include <vtkTriangle.h>
#include <vtkTransform.h>
#include <vtkPolyDataMapper.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkProperty.h>

void printPoint(const char *comment, double *p)
{
    std::cout << comment << ": " << p[0] << " "
              << p[1] << " " << p[2] << std::endl;
}

/**
 * @brief 通过三点确定一个面，并旋转到标准的平面上
 * @return
 */
void planeToNormal()
{
    double point1[3] = {1, 0, 1};
    double point2[3] = {1, 1, 0};
    double point3[3] = {0, 1, 1};
    double normal[3];
    double center[3];

    // calculate the normal with 3 points and set up for a plane
//    vtkSmartPointer<vtkPlane> plane = vtkPlane::New();
    vtkTriangle::ComputeNormal(point1, point2, point3, normal);
    std::cout << "normal: " << normal[0] << " "
              << normal[1] << " " << normal[2] << std::endl;
//    vtkTriangle::TriangleCenter(point1, point2, point3, center);
//    printPoint("center", center);
//    plane->SetOrigin(point1);
//    plane->SetNormal(normal);

    double p[3] = {1, 1, 1};

//    double projected[3];
//    double *origin = point1;
//    plane->ProjectPoint(p, origin, normal, projected);

//    std::cout << "Projected: " << projected[0] << " "
//              << projected[1] << " " << projected[2] << std::endl;

    // 旋转矩阵
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
    double normalZ[3] = {0,0,1};
    double normalA[3];
    vtkMath::Cross(normal, normalZ, normalA);
    double angle = acos(vtkMath::Dot(normalZ, normal));
    angle = vtkMath::DegreesFromRadians(angle);
    std::cout << "angle: " << angle<< std::endl;
//    transform->Translate(-1, 0, 0);
    transform->RotateWXYZ(angle, normalA);      // 旋转到和Z轴重合
    transform->Translate(-1, 0, -1);            // 设置旋转的原点

    vtkSmartPointer<vtkMatrix4x4> m =
            vtkSmartPointer<vtkMatrix4x4>::New();
    transform->GetMatrix(m);
//    std::cout << "Matrix: " << endl << *m << std::endl;

    double p_[3];  transform->TransformPoint(p, p_);
    double p1[3];  transform->TransformPoint(point1, p1);
    double p2[3];  transform->TransformPoint(point2, p2);
    double p3[3];  transform->TransformPoint(point3, p3);

    printPoint("transformed p: ", p_);
    printPoint("transformed point1: ", p1);
    printPoint("transformed point2: ", p2);
    printPoint("transformed point3: ", p3);

    double angleZ = atan2(p2[1], p2[0]);
    angleZ = vtkMath::DegreesFromRadians(angleZ);
    std::cout << "angleZ: " << angleZ<< std::endl;
    transform->Identity();
    transform->GetMatrix(m);
//    std::cout << "Matrix: " << endl << *m << std::endl;
    transform->RotateZ(-angleZ);                       // 旋转到P1和P2到X上
    std::cout << "result===" << endl;

    double pr1[3];  transform->TransformPoint(p1, pr1);
    double pr2[3];  transform->TransformPoint(p2, pr2);
    double pr3[3];  transform->TransformPoint(p3, pr3);

    printPoint("transformed point1: ", pr1);
    printPoint("transformed point2: ", pr2);
    printPoint("transformed point3: ", pr3);
}

vtkSmartPointer<vtkTransform> getTransformA(double sp1[3], double sp2[3], double sp3[3],
                                         double tp1[3], double tp2[3], double tp3[3])
{
    // 1. 计算平面法向量
    double sN[3]; double tN[3];
    vtkTriangle::ComputeNormal(sp1, sp2, sp3, sN);
    vtkTriangle::ComputeNormal(tp1, tp2, tp3, tN);

    // 2. 矩阵
    // 移动sp1 到 tp1, 绕A轴旋转
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
    double axisA[3];
    vtkMath::Cross(sN, tN, axisA);
//    printPoint("rotate axis A", axisA);
    double angleA = vtkMath::AngleBetweenVectors(sN, tN);
    angleA = vtkMath::DegreesFromRadians(angleA);
//    std::cout << "rotate A angle: " << angleA << std::endl;
    transform->PostMultiply();
    transform->Translate(-sp1[0], -sp1[1], -sp1[2]);        // 平移
    transform->RotateWXYZ(angleA, axisA);
    transform->Translate(tp1[0], tp1[1], tp1[2]);

//    vtkSmartPointer<vtkMatrix4x4> m =
//            vtkSmartPointer<vtkMatrix4x4>::New();
//    m->DeepCopy(transform->GetMatrix());

    return transform;
}


/**
 * @brief 面到面的转换，最后得到一个旋转矩阵
 * @return
 */
vtkSmartPointer<vtkTransform> getTransform(double sp1[3], double sp2[3], double sp3[3],
                                         double tp1[3], double tp2[3], double tp3[3])
{
    double axisA[3],axisB[3];
    double angleA, angleB;

    // 1. 计算平面法向量
    double sN[3]; double tN[3];
    vtkTriangle::ComputeNormal(sp1, sp2, sp3, sN);
    vtkTriangle::ComputeNormal(tp1, tp2, tp3, tN);

    // 2. 移动sp1 到 原点, 绕A轴旋转, 然后 移到  tp1
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();

    vtkMath::Cross(sN, tN, axisA);
//    printPoint("rotate axis A", axisA);
    angleA = vtkMath::AngleBetweenVectors(sN, tN);
    angleA = vtkMath::DegreesFromRadians(angleA);
//    std::cout << "rotate A angle: " << angleA << std::endl;
    transform->PostMultiply();
    transform->Translate(-sp1[0], -sp1[1], -sp1[2]);        // 平移
    transform->RotateWXYZ(angleA, axisA);
    transform->Translate(tp1[0], tp1[1], tp1[2]);

    // 此时，使得6个点都位于同一个平面上，并且 sp1 == tp1

    // 验证p2点
    double ep2[3];
    transform->TransformPoint(sp2, ep2);
//    printPoint("sp2->ep2", ep2);

    // 3. 旋转让 sp1->sp2(ep2), tp1->tp2 方向一致
    double v1[3], v2[3];
    vtkMath::Subtract(ep2, tp1, v1);
    vtkMath::Subtract(tp2, tp1, v2);

    vtkMath::Cross(v1, v2, axisB);
    angleB = vtkMath::AngleBetweenVectors(v1, v2);
    angleB = vtkMath::DegreesFromRadians(angleB);

    transform->Translate(-tp1[0], -tp1[1], -tp1[2]);        // 平移
    transform->RotateWXYZ(angleB, axisB);
    transform->Translate(tp1[0], tp1[1], tp1[2]);

    // 4. 最后的矩阵
    vtkSmartPointer<vtkMatrix4x4> m =
            vtkSmartPointer<vtkMatrix4x4>::New();
    m->DeepCopy(transform->GetMatrix());

    return transform;
}

/**
 * @brief planeToPlane 面到面的转换，最后得到一个旋转矩阵
 */
void planeToPlane()
{
    // 起始平面 和 终点平面, 注意点的顺序
    // sp1 = sp2
//    double sp1[3]= {0, 0, 1};
//    double sp2[3]= {1, 0, 0};
//    double sp3[3]= {1, 0, 1};

//    double tp1[3]= {0, 0, 1};
//    double tp2[3]= {1, 1, 1};
//    double tp3[3]= {0, 1, 1};

    // 已经在同一个面上
    double sp1[3]= {-1, -1, 1};
    double sp2[3]= {0, 0, 1};
    double sp3[3]= {-1, 0, 1};

    double tp1[3]= {0, 0, 1};
    double tp2[3]= {1, 1, 1};
    double tp3[3]= {0, 1, 1};


    // 验证p2点
    vtkSmartPointer<vtkTransform> transform = getTransform(sp1, sp2, sp3, tp1, tp2, tp3);
    double ep1[3], ep2[3], ep3[3];
    transform->TransformPoint(sp1, ep1);
    transform->TransformPoint(sp2, ep2);
    transform->TransformPoint(sp3, ep3);
    printPoint("ep1", ep1);
    printPoint("ep2", ep2);
    printPoint("ep3", ep3);
}

void displayPlanes()
{
    double sp1[3]= {-1, -1, 1};
    double sp2[3]= {0, 0, 1};
    double sp3[3]= {-1, 0, 1};

    double tp1[3]= {0, 0, 1};
    double tp2[3]= {1, 1, 1};
    double tp3[3]= {0, 1, 1};

    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPolyData> data = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPolyDataMapper> mapperS = vtkSmartPointer<vtkPolyDataMapper>::New();
    vtkSmartPointer<vtkPolyDataMapper> mapperT = vtkSmartPointer<vtkPolyDataMapper>::New();
    vtkSmartPointer<vtkActor> actorS = vtkSmartPointer<vtkActor>::New();
    vtkSmartPointer<vtkActor> actorT = vtkSmartPointer<vtkActor>::New();

    vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
    renderer->SetBackground( 0, 0, 0 );
    vtkSmartPointer<vtkRenderWindow> renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer( renderer );
    renderWindow->SetSize(800, 600);

    // do real work
    points->InsertPoint(0, sp1);
    points->InsertPoint(1, sp2);
    points->InsertPoint(2, sp3);

    points->InsertPoint(3, tp1);
    points->InsertPoint(4, tp2);
    points->InsertPoint(5, tp3);

    vtkIdType cellId1[3] = { 0, 1, 2 };
    vtkIdType cellId2[3] = { 3, 4, 5 };
    cells->InsertNextCell(3, cellId1);
    cells->InsertNextCell(3, cellId2);

    data->SetPoints(points);
    data->SetPolys(cells);

    mapperS->SetInputData(data);
    actorS->SetMapper(mapperS);
    actorS->GetProperty()->SetOpacity( 0.5 );
    actorS->GetProperty()->SetColor( 0.4, 0.2, 0.7 );
    actorT->SetMapper(mapperT);
    renderer->AddActor(actorS);
    renderer->AddActor(actorT);


    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
            vtkSmartPointer<vtkRenderWindowInteractor>::New();
    renderWindowInteractor->SetRenderWindow( renderWindow );

    // display
    renderWindow->Render();
    renderWindowInteractor->Start();
}


int main(int, char *[])
{
//    vtkSmartPointer<vtkPoints> points;
//    if (points == nullptr) {
//        std::cout << "nullptr" << std::endl;
//    }
//    planeToPlane();
    displayPlanes();

    return EXIT_SUCCESS;
}
